#include "MiniDiscreteCurve.h"

using namespace XYF;

MiniDiscreteCurve::MiniDiscreteCurve(const MiniArray<Vector3f> &points, const MiniArray<Real> &t)
    : m_t(t.size()), m_points(points)
{
    // 标准化到 [0,1] 上，先找到参数范围
    Real tmin = t[0];
    Real tmax = t[0];
    int m = m_points.size() - 1;
    for (int i = 0; i < m + 1; i++)
    {
        if (tmin > t[i])
            tmin = t[i];
        if (tmax < t[i])
            tmax = t[i];
    }

    Real dt = tmax - tmin;
    for (int i = 0; i < m + 1; i++)
        m_t[i] = (t[i] - tmin) / dt;
}

int MiniDiscreteCurve::GetType()
{
    return MiniObject::discrete_curve;
}

int MiniDiscreteCurve::GetPointsNum() const
{
    return m_points.size();
}

const MiniArray<Vector3f> &MiniDiscreteCurve::GetPoints() const
{
    return m_points;
}

MiniArray<Vector3f> &MiniDiscreteCurve::GetPoints()
{
    return m_points;
}

const Vector &MiniDiscreteCurve::GetPara() const
{
    return m_t;
}

Vector &MiniDiscreteCurve::GetPara()
{
    return m_t;
}

MiniBSplineCurve *MiniDiscreteCurve::FittingCurve(int N, int d) const
{
    // N 个控制点
    int n = N - 1;
    int m = m_points.size() - 1;

    // 构造样条
    MiniBSplineCurve *spline = new MiniBSplineCurve(n + 1, d);
    MiniArray<Vector3f> &cpts = spline->GetPoints();

    // 填充系数矩阵
    Matrix A(m + 1, n + 1);
    for (int i = 0; i < m + 1; i++)
    {
        int span = FindSpan(m_t[i], spline->GetDegree(), spline->GetKnots());
        MiniArray<Real> N = BasisFuncs(span, m_t[i], spline->GetDegree(), spline->GetKnots());
        for (int j = 0; j < d + 1; j++)
            A(i, span - d + j) = N[j];
    }

    // 求解最小二乘系统（通过取逆矩阵）
    Matrix B = (A.transpose() * A).inverse() * A.transpose();
    for (int i = 0; i < n + 1; i++)
    {
        for (int k = 0; k < m + 1; k++)
            cpts[i] = cpts[i] + m_points[k] * B(i, k);
    }
    return spline;
}

MiniBSplineCurve *MiniDiscreteCurve::LSPIACurve(int N, int d, Real tol) const
{
    // N 个控制点，m + 1 个数据点和对应参数
    int n = N - 1;
    int m = m_points.size() - 1;

    // 构造样条
    MiniBSplineCurve *spline = new MiniBSplineCurve(n + 1, d);
    MiniArray<Vector3f> &P = spline->GetPoints();
    const MiniArray<Vector3f> &Q = GetPoints();

    // 几何迭代法
    Real e = tol * 10;
    while (e > tol && n < N * 2)
    {
        n = spline->GetPointsNum() - 1;

        MiniArray<Real> basisSum(n + 1);      // 记录每个控制点对应的那组基函数和
        MiniArray<Vector3f> deltaSum(n + 1);  // 记录每个控制点对应的那组偏差向量加权和
        MiniArray<Real> dSum(n - d + 1);      // 记录每个区间段上的累计误差
        MiniArray2D<Real> paraSet(n - d + 1); // 记录每个区间段上的参数

        // 对每个数据点计算增量
        for (int i = 0; i < m + 1; i++)
        {
            Vector3f delta = Q[i] - spline->CurvePoint(m_t[i]);

            // 计算此参数对应的非零基函数值
            int j = FindSpan(m_t[i], spline->GetDegree(), spline->GetKnots());
            MiniArray<Real> basisFunc = BasisFuncs(j, m_t[i], spline->GetDegree(), spline->GetKnots());

            // 累计参数
            for (int k = 0; k < int(basisFunc.size()); k++)
            {
                basisSum[j - d + k] += basisFunc[k];
                deltaSum[j - d + k] = deltaSum[j - d + k] + delta * basisFunc[k];
            }
        }

        // 得到控制点增量的平均值并加上增量
        for (int i = 0; i < n + 1; i++)
            P[i] = P[i] + deltaSum[i] / basisSum[i];

        // 计算区间误差
        e = 0;
        for (int i = 0; i < m + 1; i++)
        {
            Vector3f delta = Q[i] - spline->CurvePoint(m_t[i]);
            Real di = length(delta);

            // 计算此参数对应的区间段，然后累计误差
            int j = FindSpan(m_t[i], spline->GetDegree(), spline->GetKnots());
            dSum[j - d] += di;
            paraSet[j - d].push_back(m_t[i]);
            e += pow(di, 2);
        }

        // 找到具有最大误差的区间段，在此区间上插入节点
        Real maxD = 0;
        int maxId = -1;
        for (int i = 0; i < int(dSum.size()); i++)
        {
            // 要求此区间段上至少有两个参数点
            if (dSum[i] > maxD && paraSet[i].size() > 1)
            {
                maxD = dSum[i];
                maxId = i;
            }
        }

        // 在 maxId 区间段上平分误差的两个相邻参数点之间插入节点
        if (maxId != -1)
        {
            int j = paraSet[maxId].size() / 2;
            spline->CurveKnotIns((paraSet[maxId][j] + paraSet[maxId][j - 1]) / 2, 1);
        }
    }
    return spline;
}
